/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package mygame.procedural_walk;

import com.jme3.math.Vector3f;
import mygame.procedural.ik.LegControl;

/**
 *
 * @author Paulo
 */
public class GaitManager {
    
    private float needle;
    private float[] start;
    private LegControl[] legs;
    private Vector3f velocity;
    private Vector3f dv;
    private float cycleDistance;
    
    public GaitManager(float[] start, LegControl[] legs) {
        
        needle = 0.0f;
        this.start = start;
        this.legs = legs;
        
        velocity = new Vector3f();
        dv = new Vector3f();
    }
    
    public void setVelocity(Vector3f velocity) {
        this.dv = velocity.subtract(this.velocity);
        this.velocity = velocity.clone();
    }
    
    private boolean legOnNeedle(int leg) {
        boolean invert = false;
        float duration = 1f / start.length;
        float normStart = start[leg];
        float normEnd = start[leg] + duration;
        
        if (normEnd > 1.0) {
            normEnd -= 1.0;
            invert = true;
        }
        
        if (invert) {
            return needle >= normStart || needle < normEnd;
        } else {
            return needle >= normStart && needle < normEnd;
        }
    }
    
    public void update(float tpf) {
        needle += tpf * velocity.length();
        while (needle > 1.0f) {
            needle -= 1.0f;
        }
        
        for (int i = 0; i < legs.length; i++) {
            if (legOnNeedle(i)) {
                legs[i].swing();
            }
        }
    }
}
